﻿// 近似坐标计算：包括自由测站边角交会网、交叉导线网、混合网、特殊网等
// 计算方法：坐标转换、方位角+距离推算、测角交会、测边交会、后方交会等
using System;
using System.Collections.Generic;
using System.Text;
using MathNet.Numerics.LinearAlgebra;
using static AdjPlane.Angle_t;
using static AdjPlane.AdjCmn;
using System.IO;

namespace AdjPlane
{
    using poi_dic = Dictionary<string, Point2D>;

    // 存储近似坐标，避免字典链表等类型的浅拷贝
    public struct Point_t
    {
        public double x;
        public double y;
        public string name;
    }
    // 存储计算近似坐标所围成的三个点组成的三角形
    public struct Triangle_t
    {
        public string Aname;
        public string Bname;
        public string Pname;
        public int sign;  // 1表示ABP顺时针，-1表示逆时针
    }
    /* 圆周穷举法推算近似坐标需要用到 */
    public struct LoopX
    {
        public int num;
        public double x;
        public double y;
        public double eps;
    }
    //存储推算近似坐标时的近似方位角
    public struct Azi_t
    {
        public string beg;
        public string end;
        public double azi;
    }

    //计算概略坐标
    public partial class Plane
    {
        protected Point_t[] CopyApprox;  //Approx到Result是浅拷贝，前者会随着后者改变，所以需要存储初值
        protected List<Azi_t> Azis = new List<Azi_t>();//存储近似方位角，用于距离方位角坐标推算
        // 读取测边交会二义性文件
        void ReadApprox_3S(string path,List<Triangle_t> S3)
        {
            Encoding.RegisterProvider(CodePagesEncodingProvider.Instance);//注册gb2312  
            string[] line = File.ReadAllLines(path, Encoding.GetEncoding("gb2312"));
            for(int i = 0; i < line.Length; i++)
            {
                if (line[i].Contains("#"))
                    continue;
                string[] str = mysplit(line[i], splitCol);
                Triangle_t s3 = new Triangle_t();
                s3.Aname = str[0];
                s3.Bname = str[1];
                s3.Pname = str[2];
                s3.sign = Convert.ToInt32(str[3]);
                S3.Add(s3);
            }
        }
        int FindApprox_3S(string Aname,string Bname,string Pname,List<Triangle_t> S3)
        {
            for(int i = 0; i < S3.Count; i++)
            {
                if ((Aname == S3[i].Aname && Bname == S3[i].Bname
                    && Pname == S3[i].Pname) ||
                    (Aname == S3[i].Bname && Bname == S3[i].Pname
                    && Pname == S3[i].Aname) ||
                    (Aname == S3[i].Pname && Bname == S3[i].Aname
                    && Pname == S3[i].Bname) )
                    return S3[i].sign;
            }
            return 0;
        }
        // 距离交会二义性存储  S3 :存储二义性的三点
        void Approx_3S(string Aname,string Bname,string Pname,int sign,List<Triangle_t> S3)
        {
            Triangle_t s3 = new Triangle_t();
            s3.Aname = Aname;
            s3.Bname = Bname;
            s3.Pname = Pname;
            s3.sign = sign;
            int m,k;
            for (m = 0; m < S3.Count; m++)
            {
                k = 0;
                if (Aname == S3[m].Aname || Aname == S3[m].Bname || 
                    Aname == S3[m].Pname)
                    k++;
                if (Bname == S3[m].Aname || Bname == S3[m].Bname ||
                    Bname == S3[m].Pname)
                    k++;
                if (Pname == S3[m].Aname || Pname == S3[m].Bname ||
                    Pname == S3[m].Pname)
                    k++;
                if (k==3)
                    break;
            }
            if (S3.Count == 0 || m == S3.Count)
            {
                S3.Add(s3);
            }
        }

        // 角度前方交会，sign=1/-1，表示测站A中，ABP顺/逆时针排列
        static Point2D Forward_Intersection(Point2D A, Point2D B, double alpha, double beta, int sign)
        {
            double cota = 1.0 / Math.Tan(alpha);
            double cotb = 1.0 / Math.Tan(beta);

            Point2D P = new Point2D();
            if (sign == 1)    //azi_ap  = azi_ab + ∠BAP
            {
                P.X = (A.X * cotb + B.X * cota - (B.Y - A.Y)) / (cota + cotb);
                P.Y = (A.Y * cotb + B.Y * cota + (B.X - A.X)) / (cota + cotb);
            }
            if (sign == -1)   //azi_ap  = azi_ab - ∠BAP
            {
                P.X = (A.X * cotb + B.X * cota + (B.Y - A.Y)) / (cota + cotb);
                P.Y = (A.Y * cotb + B.Y * cota - (B.X - A.X)) / (cota + cotb);
            }
            return P;
        }
        //测边交会,sign=1/-1，表示测站A中，ABP顺/逆时针排列
        static Point2D Trilateral_Intersection(Point2D A, Point2D B, double Sap, double Sbp, int sign)
        {
            double dx = B.X - A.X;
            double dy = B.Y - A.Y;
            double azi_ab = GetAzi(dx, dy);
            double Sab = Math.Sqrt(dx * dx + dy * dy);
            double cosa = (Sab * Sab + Sap * Sap - Sbp * Sbp) / (2 * Sap * Sab);//余弦定理
            double azi_ap;
            if (cosa < -1 || cosa > 1)
                return new Point2D();

            // sign= 1, azi_ap = azi_ab + ∠BAP
            // sign=-1, azi_ap = azi_ab - ∠BAP
            if (sign == 1) azi_ap = Keep_0To2Pi(azi_ab + Math.Acos(cosa));
            else azi_ap = Keep_0To2Pi(azi_ab - Math.Acos(cosa));

            Point2D P = new Point2D
            {
                X = A.X + Sap * Math.Cos(azi_ap),
                Y = A.Y + Sap * Math.Sin(azi_ap),
                valid = 1
            };
            return P;
        }
        //后方交会(仿权公式)
        static Point2D Backward_Intersection(Point2D A, Point2D B, Point2D C, double pa, double pb, double pc)
        {
            Point2D P = new Point2D();
            P.X = (pa * A.X + pb * B.X + pc * C.X) / (pa + pb + pc);
            P.Y = (pa * A.Y + pb * B.Y + pc * C.Y) / (pa + pb + pc);
            return P;
        }
        // 调整观测值顺序 C#中，字典链表等类型传递，默认ref,引用传递
        void AdjustObs(List<Sta> AllObs)
        {
            int k = 0;
            if (knownPoint.ContainsKey(AllObs[0].name))
                k++;
            for (int i = 0; i < AllObs[0].nl; i++)
            {
                if (knownPoint.ContainsKey(AllObs[0].data[i].name))
                    k++;
            }
            if (k == 0)
            {
                // 如果第一测站没有已知点，那么就在最后一个测站（隧道网一般是这样）
                AllObs.Reverse();
            }
        }
        //复制近似坐标
        protected static Point_t[] CopyDic(poi_dic Approx)
        {
            int len = Approx.Count;
            Point_t[] CopyApprox = new Point_t[len];
            int i = 0;
            foreach (KeyValuePair<string, Point2D> p in Approx)
            {
                //C#实例化自定义数组，必须实例化数组中的每一个类，否则NULL
                CopyApprox[i] = new Point_t
                {
                    x = p.Value.X,
                    y = p.Value.Y,
                    name = p.Key
                };
                i++;
            }
            return CopyApprox;
        }
        //测站索引
        protected int FindStaName(string name)
        {
            int i;
            for (i = 0; i < AllObs.Count; i++)
            {
                if (name == AllObs[i].name)
                    return i;
            }
            return -1;
        }
        //照准点索引
        protected int FindObsName(int id, string name)
        {
            int i;
            for (i = 0; i < AllObs[id].data.Count; i++)
            {
                if (name == AllObs[id].data[i].name)
                    return i;
            }
            return -1;
        }
        //方位角索引
        int FindAzi(string beg,string end,List<Azi_t> azi0)
        {
            for(int i = 0; i < azi0.Count; i++)
            {
                if (beg == azi0[i].beg && end == azi0[i].end)
                    return i;
            }
            return -1;
        }
        //利用已有的近似坐标反算本侧站中的方位角
        void CalAzi(string staName, List<int> Index, List<Azi_t> Azis)
        {
            if (!Approx.ContainsKey(staName))
                return;
            int i = FindStaName(staName);
            var data = AllObs[i].data;
            if (Index.Count == 0) return;
            int idk = Index[0];
            string obsKnName = data[idk].name;//已知的照准点
            double dx0 = Approx[obsKnName].X - Approx[staName].X;
            double dy0 = Approx[obsKnName].Y - Approx[staName].Y;
            double azi0 = GetAzi(dx0, dy0);
            if (FindAzi(staName, obsKnName, Azis) == -1)
            {
                Azi_t az = new Azi_t();
                az.beg = staName;
                az.end = obsKnName;
                az.azi = azi0;
                Azis.Add(az);
                if (FindAzi(obsKnName, staName, Azis) == -1)
                {
                    az = new Azi_t();
                    az.beg = obsKnName;
                    az.end = staName;
                    az.azi = Keep_0To2Pi(azi0 + Math.PI);
                    Azis.Add(az);
                }
            }
            for (int j = 0; j < data.Count; j++)
            {
                if (j == idk) continue;
                if (AngleExist(i, idk) == 0) break;
                if (AngleExist(i, j) == 0) continue;
                double azi = Keep_0To2Pi(azi0 + DMS2RD(Angle(i, j)) - DMS2RD(Angle(i, idk)));
                if (FindAzi(staName, data[j].name, Azis) == -1)
                {
                    Azi_t az = new Azi_t();
                    az.beg = staName;
                    az.end = data[j].name;
                    az.azi = azi;
                    Azis.Add(az);
                    if (FindAzi(data[j].name, staName, Azis) == -1)
                    {
                        az = new Azi_t();
                        az.beg = data[j].name;
                        az.end = staName;
                        az.azi = Keep_0To2Pi(azi + Math.PI);
                        Azis.Add(az);
                    }
                }
            }
        }
        //是否存在角度观测值
        int AngleExist(int i,int j)
        {
            if (j != 0 && AllObs[i].data[j].angle == 0)
                return 0;
            return 1;
        }
        //求照准方向ij与ik照准过程扫过的角度
        //不管先照准ij还是ik，结果都是从ij顺时针到ik
        //注意：如果ijk顺时针排列 ，结果始终为0~180°
        //如果ijk逆时针排列，结果始终180~360°
        double Angle_ijk(int i, int j, int k)
        {
            double alpha = 0;
            if (AngleExist(i, j) == 1 && AngleExist(i, k) == 1) //两个照转方向都有L观测值
            {
                double aik = Angle(i, k);
                double aij = Angle(i, j);
                if (j < k)
                    alpha = DMS2RD(aik) - DMS2RD(aij);  //ik-ij
                else
                    alpha = 2 * Math.PI - (DMS2RD(aij) - DMS2RD(aik));
            }
            return alpha;
        }
        double Angle_ijk(string Aname,string Bname,string Pname)
        {
            int i = FindStaName(Aname);
            if (i != -1)
            {
                int j = FindObsName(i, Bname);
                int k = FindObsName(i, Pname);
                if (j != -1 && k != -1)
                    return Angle_ijk(i, j, k);
            }
            return 0;
        }
        //ABP顺时针排列，AB->AP顺时针扫过的角度 (0,180°)
        double Angle_ijk(Point2D A, Point2D B, Point2D P)
        {
            double azi_ab = GetAzi(A, B);
            double azi_ap = GetAzi(A, P);
            return Angle_ijk(azi_ab, azi_ap);
        }
        //ij沿顺时针方向扫到ik的夹角
        //注意：得到的结果区间 [0,360°)
        double Angle_ijk(double azi_ij,double azi_ik)
        {
            double ag = 0;
            if ((Math.PI - azi_ij) * (Math.PI - azi_ik) > 0)
                ag = azi_ik - azi_ij; 
            else if ((Math.PI - azi_ij) * (Math.PI - azi_ik) <= 0)
            {
                if (azi_ij > Math.PI)
                    ag = azi_ik - azi_ij + 2 * Math.PI;
                else if(azi_ij < Math.PI)
                    ag = azi_ik - azi_ij;
                else
                {
                    if (azi_ik >= Math.PI) ag = azi_ik - azi_ij;
                    else ag = azi_ik - azi_ij + 2 * Math.PI;
                }
            }
            return ag;
        }
        //方位角的夹角[0,90°)
        double AziDiff(double azi_ij,double azi_ik)
        {
            double ag = Angle_ijk(azi_ij, azi_ik);
            if (ag > Math.PI * 0.5 || ag < Math.PI * 1.5)
                ag = Math.Abs(Math.PI - ag);
            if (ag > Math.PI * 1.5)
                ag = Math.PI * 2 - ag;
            return ag;
        }
        double Angle(int i,int j)
        {
            return AllObs[i].data[j].angle;
        }
        double Angle(string staName,string obsName)
        {
            int ids = FindStaName(staName);
            if (ids != -1)
            {
                int ido = FindObsName(ids, obsName);
                if (ido != -1)
                {
                    return Angle(ids, ido);
                }
            }
            return 0;
        }
        double Dis(int i,int j)
        {
            return AllObs[i].data[j].distance;
        }
        double Dis(string staName, string obsName)
        {
            int ids = FindStaName(staName);
            if (ids != -1)
            {
                int ido = FindObsName(ids, obsName);
                if (ido != -1)
                {
                    return Dis(ids, ido);
                }
            }
            return 0;
        }
        double Dis(Point2D A,Point2D B)
        {
            double dx = A.X - B.X;
            double dy = A.Y - B.Y;
            return Math.Sqrt(dx * dx + dy * dy);
        }
        //S是否照准O，S的测站序号，O 的照准点编号
        int Aim(string staName, string obsName, ref int ids, ref int ido)
        {
            ids = FindStaName(staName);
            if (ids != -1)
            {
                ido = FindObsName(ids, obsName);
                if (ido != -1)
                    return 1;
            }
            return 0;
        }
        int Aim(string staName, string obsName)
        {
            int ids = FindStaName(staName);
            if (ids != -1)
            {
                int ido = FindObsName(ids, obsName);
                if (ido != -1)
                    return 1;
            }
            return 0;
        }
        //如果测边交会不知道ABP顺逆时针，利用交会结果与测站上的其他点比较（如果有的话）
        int PointTrue(string Pname, Point2D P, ref double eps)
        {
            int ids = FindStaName(Pname);
            if (ids == -1) return -1;   //无法验证，因为不能存在这个测站，自然没有多余的测距

            var data = AllObs[ids].data;
            List<string> Names = new List<string>();
            int i, num = 0;
            for (i = 0; i < data.Count; i++)
            {
                string name = data[i].name;
                double dis = Dis(ids, i);
                if (dis == 0 && Aim(name, Pname) == 1)
                    dis = Dis(name, Pname);
                if (Approx.ContainsKey(name) && dis != 0)
                {
                    Names.Add(name);
                    num++;
                    Point2D np = Approx[name];
                    double dis0 = Dis(P, np);
                    eps += Math.Abs(dis - dis0);
                }
            }
            double azibias = 0;
            Point2D A = Approx[Names[0]];
            for (i = 1; i < num - 1; i++)
            {
                Point2D B = Approx[Names[i]];
                Point2D C = Approx[Names[i + 1]];
                double azi_ab = GetAzi(A, B);
                double azi_ac = GetAzi(A, C);
                double ag = AziDiff(azi_ab, azi_ac);
                azibias += ag;
            }
            if (num < 3) return -1;  //只有两个点，用于测边交会的点，没有多余测距，无法验证
            if (azibias < 1e-3) return -1; // 所有点几乎都在一条线上，测边交会的两个点到其余所有点的距离相等,无法验证。
            if (eps > 1) return 0;  //距离偏差大于1m，说明要么是另一个点，要么用于测边交会的已知点有问题
            return 1;
        }
        //边边角的情况，穷举法
        //以已知点为圆心，以距离为半径画圆，遍历圆上所有点，直至与另一个已知点的夹角等于测角
        int LoopCircle(string Pname, ref Point2D Poi)
        {
            int ids = FindStaName(Pname);
            if (ids == -1) return 0;
            List<string> K = new List<string>();
            var data = AllObs[ids].data;
            int ns = 0;
            for (int i = 0; i < data.Count; i++)
            {
                string name = data[i].name;
                if (Approx.ContainsKey(name))
                    K.Add(name);
                if (Dis(Pname, name) == 0)
                {
                    if (Aim(name, Pname) == 1)
                        if (Dis(name, Pname) != 0)
                            ns++;
                }
                else
                    ns++;
            }

            int len = K.Count;
            if (len < 2) return 0;
            if (len < ns) return 0;  //等已知测距的照准点都计算出来了，再计算。

            string Aname, Bname;
            int idj = -1, idk = -1; //idj测距角度已知，idk角度已知
            double Xj = 0, Yj = 0, Xk = 0, Yk = 0;
            double dis = 0, alpha = 0;  //只需要一条边，一个角
            //找测距
            for (int i = 0; i < len; i++)
            {
                string name = K[i];
                int ido = FindObsName(ids, name);

                if (Dis(ids, ido) != 0)
                    dis = Dis(ids, ido);
                else
                    if (Aim(name, Pname) == 1)
                    dis = Dis(name, Pname);
                if (dis != 0)
                {
                    idj = ido;
                    Xj = Approx[name].X;
                    Yj = Approx[name].Y;
                    Aname = name;
                    break;
                }
            }
            //找角度
            for (int i = 0; i < len; i++)
            {
                string name = K[i];
                int ido = FindObsName(ids, name);
                if (ido == idj) continue;
                if (idj != -1)
                    alpha = Angle_ijk(ids, idj, ido);
                if (alpha != 0)
                {
                    idk = ido;
                    Xk = Approx[name].X;
                    Yk = Approx[name].Y;
                    Bname = name;
                    break;
                }
            }
            if (dis == 0 || alpha == 0) return 0;

            int sign;
            double x, y, dx, dy, azi_pa, azi_pb, beta, eps;
            double xmin = (int)(Xj - dis), xmax = (int)(Xj + dis);
            List<double> Eps = new List<double>();
            List<LoopX> Lps = new List<LoopX>();
            int c = 0;
            sign = 1;
            do
            {
                for (x = xmin; x <= xmax; x += 1)
                {
                    LoopX lp = new LoopX();
                    dx = Xj - x;
                    if (dis < Math.Abs(dx)) continue;
                    dy = sign * Math.Sqrt(dis * dis - dx * dx);
                    y = Yj + dy;
                    azi_pa = GetAzi(Xj - x, Yj - y);
                    azi_pb = GetAzi(Xk - x, Yk - y);
                    beta = Angle_ijk(azi_pa, azi_pb);
                    eps = Math.Abs(alpha - beta) * rou;
                    Eps.Add(eps);
                    lp.num = c;
                    lp.x = x;
                    lp.y = y;
                    lp.eps = eps;
                    Lps.Add(lp);
                    c++;
                }
                sign *= -1;
            } while (sign == -1);
            double Epsmin = 0; int idx = 0;
            for (int i = 0; i < c; i++)
            {
                if (i == 0) { Epsmin = Lps[0].eps; idx = i; }
                if (Epsmin > Lps[i].eps) { Epsmin = Lps[i].eps; idx = i; }
            }

            xmin = Lps[idx].x - 1;
            xmax = xmin + 2;
            Lps = new List<LoopX>();
            c = 0;
            Eps = new List<double>();
            sign = 1;
            do
            {
                for (x = xmin; x <= xmax; x += 0.01)
                {
                    LoopX lp = new LoopX();
                    dx = Xj - x;
                    if (dis < Math.Abs(dx)) continue;
                    dy = sign * Math.Sqrt(dis * dis - dx * dx);
                    y = Yj + dy;
                    azi_pa = GetAzi(Xj - x, Yj - y);
                    azi_pb = GetAzi(Xk - x, Yk - y);
                    beta = Angle_ijk(azi_pa, azi_pb);
                    eps = Math.Abs(alpha - beta) * rou;
                    Eps.Add(eps);
                    lp.num = c;
                    lp.x = x;
                    lp.y = y;
                    lp.eps = eps;
                    Lps.Add(lp);
                    c++;
                }
                sign *= -1;
            } while (sign == -1);

            Epsmin = 0; idx = 0;
            for (int i = 0; i < c; i++)
            {
                if (i == 0) { Epsmin = Lps[0].eps; idx = i; }
                if (Epsmin > Lps[i].eps) { Epsmin = Lps[i].eps; idx = i; }
            }

            x = Lps[idx].x;
            y = Lps[idx].y;
            Poi = new Point2D
            {
                X = x,
                Y = y
            };
            return 1;
        }
        //自由测站法 (弃用：：史赖伯，消定向角)
        //为什么测边交会不收敛啊
        int FreeStation(string Pname, ref Point2D Poi)
        {
            int ids = FindStaName(Pname);
            if (ids == -1) return 0;
            if (AllObs[ids].nl > 0) return 0;
            if (AllObs[ids].ns < 3) return 0;  //

            List<string> K = new List<string>();
            var data = AllObs[ids].data;
            for (int i = 0; i < data.Count; i++)
            {
                if (Approx.ContainsKey(data[i].name))
                    K.Add(data[i].name);
            }
            int len = K.Count;
            if (len < 3) return 0;

            var A1 = Newmat(len + 1, 2);
            var L1 = Newmat(len + 1, 1);
            var P1 = Newmat(len + 1, len + 1);

            var A2 = Newmat(len, 2);
            var L2 = Newmat(len, 1);
            var P2 = Newmat(len, len);

            var X = Newmat(2, 1);
            var dx = Newmat(2, 1);

            double DXJ = 0, Xj, Yj, Xk, Yk, S, azi;
            const double cr = rou / 3600;

            double x0 = 0, y0 = 0;
            for (int i = 0; i < len; i++)
            {
                string name = K[i];
                x0 += Approx[name].X / len;
                y0 += Approx[name].Y / len;
            }

            for (int iter = 0; iter < 8; iter++)
            {
                int nl = 0, ns = 0;
                double asum = 0, bsum = 0, lsum = 0;
                for (int k = 0; k < len; k++)
                {
                    string name = K[k];
                    int ido = FindObsName(ids, name);

                    Xk = Approx[name].X;
                    Yk = Approx[name].Y;
                    if (iter == 0)
                    {
                        X[0, 0] = 19800;
                        X[1, 0] = 46918;
                    }
                    Xj = X[0, 0];
                    Yj = X[1, 0];

                    if (k == -1)
                    {
                        double sum = 0;
                        for (int i = 0; i < len; i++)
                        {
                            string iname = K[i];
                            int iido = FindObsName(ids, iname);
                            double ix = Approx[iname].X;
                            double iy = Approx[iname].Y;
                            double iazi = GetAzi(ix - Xj, iy - Yj);
                            double obsAngle = DMS2RD(Angle(ids, iido));
                            sum += Keep_0To2Pi(iazi - obsAngle);
                        }
                        DXJ = sum / len;
                    }

                    S = Math.Sqrt(Math.Pow(Xk - Xj, 2) + Math.Pow(Yk - Yj, 2));
                    azi = GetAzi(Xk - Xj, Yk - Yj);

                    if (AllObs[ids].nl != 0 && AngleExist(ids, ido) == 1)
                    {
                        double a = cr * (Yk - Yj) / S / S;
                        double b = cr * (Xk - Xj) / S / S;
                        double l = Keep_0To360(DMS2D(Angle(ids, ido)) + DXJ) - azi;
                        asum += a; bsum += b; lsum += l;
                        A1[nl, 0] = a;
                        A1[nl, 1] = -1 * b;
                        L1[nl, 0] = l;
                        P1[nl, nl] = 1;
                        nl++;
                    }
                    if (Dis(ids, ido) != 0)
                    {
                        A2[ns, 0] = -(Xk - Xj) / S;
                        A2[ns, 1] = (Yk - Yj) / S;
                        L2[ns, 0] = Dis(ids, ido) - S;
                        P2[ns, ns] = Math.Pow(alpha / (msa + msb * Dis(ids, ido)), 2);
                        ns++;
                    }
                }
                if (AllObs[ids].nl != 0)
                {
                    A1[nl, 0] = asum;
                    A1[nl, 1] = bsum;
                    L1[nl, 0] = lsum;
                    P1[nl, nl] = -1 / nl;
                }
                if (ns < 3) return 0;

                //var A = A1.Stack(A2);
                //var P = P1.DiagonalStack(P2);
                //var L = L1.Stack(L2);
                var A = A2;
                var L = L2;
                dx = (A.Transpose() * A).Inverse() * A.Transpose() * L;
                X += dx;
                double[] arr = ColArray(dx.ToArray());
                if (Dot(arr, arr, 2) < 1e-3)
                {
                    Poi = new Point2D
                    {
                        X = X[0, 0],
                        Y = X[1, 0]
                    };
                    return 1;
                }
            }
            return 0;
        }
        /// <summary>
        /// 平面坐标4参数转换(站心坐标到假设坐标系、假设坐标系转换到真实坐标) x,y,mcosa,msina
        /// </summary>
        /// <param name="prev">上一个测站（参考坐标系）</param>
        /// <param name="next">下一个测站（待转换的坐标系）</param>
        /// <returns>待转换的坐标系(next)转换到参考坐标系(prev)下的结果</returns>
        protected static poi_dic Trans(poi_dic prev, poi_dic next)
        {
            //SortedList<T>可以对键的集合进行索引，Dictionary不行
            //V=B*X-L；VTPV=min;==>X=(BTPB)'*(BTPL)
            poi_dic result = new poi_dic();//存储转换后测站内的点
            poi_dic Sprev = new poi_dic();//重合点在上个坐标系的坐标
            poi_dic Snext = new poi_dic();//重合点在下个坐标系的坐标
            foreach (KeyValuePair<string, Point2D> item in prev)
            {
                if (next.ContainsKey(item.Key))
                    Sprev.Add(item.Key, item.Value);
            }
            foreach (KeyValuePair<string, Point2D> item in next)
            {
                if (Sprev.ContainsKey(item.Key))
                    Snext.Add(item.Key, item.Value);
            }
            if (Sprev.Count < 2) 
                return result;

            var A = Matrix<double>.Build.Dense(2 * Snext.Count, 4, 0);//平面坐标转换系数阵
            var L = Matrix<double>.Build.Dense(2 * Snext.Count, 1, 0);//观测值(常数项）
            var X = Matrix<double>.Build.Dense(4, 1, 0);//参数迭代值，初值设为零

            int i = 0;
            foreach (KeyValuePair<string, Point2D> kv in Snext)
            {
                A[2 * i, 0] = 1;
                A[2 * i, 1] = 0;
                A[2 * i, 2] = kv.Value.X;
                A[2 * i, 3] = kv.Value.Y;
                A[2 * i + 1, 0] = 0;
                A[2 * i + 1, 1] = 1;
                A[2 * i + 1, 2] = kv.Value.Y;
                A[2 * i + 1, 3] = -1 * kv.Value.X;
                L[2 * i, 0] = Sprev[kv.Key].X;
                L[2 * i + 1, 0] = Sprev[kv.Key].Y;
                i++;
            }
            var AT = A.Transpose();
            X = ((AT * A).Inverse() * AT * L);

            double delta_x = X[0, 0];
            double delta_y = X[1, 0];
            double mcosa = X[2, 0];
            double msina = X[3, 0];

            double[,] n1 = { { delta_x }, { delta_y } };//平移
            double[,] n2 = { { mcosa, msina },
                { -1 * msina, mcosa } };//缩放旋转
            var N1 = Matrix<double>.Build.DenseOfArray(n1);
            var N2 = Matrix<double>.Build.DenseOfArray(n2);

            //把第二测站的坐标全转换到第一个测站的坐标系中
            foreach (KeyValuePair<string, Point2D> kv in next)
            {
                double[,] n3 = { { kv.Value.X }, { kv.Value.Y } };
                var N3 = Matrix<double>.Build.DenseOfArray(n3);
                var N4 = N1 + N2 * N3; //转换后的坐标
                if (!result.ContainsKey(kv.Key))
                    result.Add(kv.Key, new Point2D(N4[0, 0], N4[1, 0]));
            }
            return result;
        }
        /// <summary>
        /// 四参数转换；两个平移因子改正数，缩放、旋转因子改正数
        /// </summary>
        /// <param name="prev">上一个测站（参考坐标系）</param>
        /// <param name="next">下一个测站（待转换的坐标系）</param>
        /// <param name="C">转换4参数 x,y,k,theta</param>
        /// <returns>转换后的坐标</returns>
        protected poi_dic CoordinateTrans4(poi_dic prev, poi_dic next, out double[] C)
        {
            //SortedList<T>可以对键的集合进行索引，Dictionary不行
            //V=B*X-L；VTPV=min;==>X=(BTPB)'*(BTPL)

            poi_dic result = new poi_dic();//存储转换后测站内的点
            poi_dic Sprev = new poi_dic();//重合点在上个坐标系的坐标
            poi_dic Snext = new poi_dic();//重合点在下个坐标系的坐标
            foreach (KeyValuePair<string, Point2D> item in prev)
            {
                if (next.ContainsKey(item.Key))
                    Sprev.Add(item.Key, item.Value);
            }
            foreach (KeyValuePair<string, Point2D> item in next)
            {
                if (Sprev.ContainsKey(item.Key))
                    Snext.Add(item.Key, item.Value);
            }
            var A = Matrix<double>.Build.Dense(2 * Snext.Count, 4, 0);//平面坐标转换系数阵
            var L = Matrix<double>.Build.Dense(2 * Snext.Count, 1, 0);//观测值(常数项）
            var X = Matrix<double>.Build.Dense(4, 1, 0);//参数迭代值，初值设为零

            double delta_x0 = 0;//平移因子初值
            double delta_y0 = 0;
            double theta0 = 0;//旋转因子初值
            double k0 = 0;//尺度因子初值
            double cosa = 0;
            double sina = 0;
            int count = 0;//迭代次数
            Matrix<double> X_prev;
            while (true)
            {
                int i = 0;
                foreach (KeyValuePair<string, Point2D> kv in Snext)
                { //（重合点在Sprev和Snext中的排序可能不一样） <-- 胡说八道
                    double x0 = kv.Value.X;
                    double y0 = kv.Value.Y;
                    cosa = Math.Cos(theta0);
                    sina = Math.Sin(theta0);

                    double a1 = x0 * cosa + y0 * sina;
                    double a2 = (1 + k0) * (-x0 * sina + y0 * cosa);
                    double a3 = -x0 * sina + y0 * cosa;
                    double a4 = (1 + k0) * (-x0 * cosa - y0 * cosa);

                    A[2 * i, 0] = 1;
                    A[2 * i, 1] = 0;
                    A[2 * i, 2] = a1;
                    A[2 * i, 3] = a2;
                    A[2 * i + 1, 0] = 0;
                    A[2 * i + 1, 1] = 1;
                    A[2 * i + 1, 2] = a3;
                    A[2 * i + 1, 3] = a4;

                    double x1 = Sprev[kv.Key].X;
                    double y1 = Sprev[kv.Key].Y;
                    L[2 * i, 0] = x1 - delta_x0 - (1 + k0) * a1;
                    L[2 * i + 1, 0] = y1 - delta_y0 - (1 + k0) * a3;
                    i++;
                }
                var AT = A.Transpose();
                var x = ((AT * A).Inverse() * AT * L);
                X_prev = X;
                X += x;//参数值迭代
                var dX = X - X_prev;
                double eps = 0;
                for (int j = 0; j < 4; j++)
                {
                    double dx = Math.Abs(dX[j, 0]);
                    if (eps < dx)
                        eps = dx;
                }
                count++;
                if (eps < 1e-5)
                    break;
                delta_x0 = X[0, 0];
                delta_y0 = X[1, 0];
                k0 = X[2, 0];
                theta0 = X[3, 0];
                cosa = Math.Cos(theta0);
                sina = Math.Sin(theta0);
            }

            double[,] n1 = { { delta_x0 }, { delta_y0 } };//平移
            double[,] n2 = { { cosa, sina }, { -sina, cosa } };//旋转
            var N1 = Matrix<double>.Build.DenseOfArray(n1);
            var N2 = Matrix<double>.Build.DenseOfArray(n2);
            //把第二测站的坐标全转换到第一个测站的坐标系中
            foreach (KeyValuePair<string, Point2D> kv in next)
            {
                double[,] n3 = { { kv.Value.X }, { kv.Value.Y } };
                var N3 = Matrix<double>.Build.DenseOfArray(n3);
                var N4 = N1 + (1 + k0) * N2 * N3; //转换后的坐标
                if (!result.ContainsKey(kv.Key))
                    result.Add(kv.Key, new Point2D(N4[0, 0], N4[1, 0]));
            }
            C = new double[4];
            C[0] = delta_x0;
            C[1] = delta_y0;
            C[2] = k0;
            C[3] = theta0;
            return result;
        }
        /// <summary>
        /// 在当前假设坐标系的所有测站Suppose中寻找与next关联的测站
        /// </summary>
        /// <param name="next">待转换的测站</param>
        /// <param name="Suppose">当前假设坐标系的所有测站</param>
        /// <returns></returns>
        protected static poi_dic Connect(poi_dic next, List<poi_dic> Suppose)
        {
            var target = new poi_dic();
            for (int i = Suppose.Count - 1; i >= 0; i--)
            {
                int k = 0;
                foreach (KeyValuePair<string, Point2D> p in Suppose[i])
                {
                    if (next.ContainsKey(p.Key))
                        k++;
                }
                if (k >= 2)
                {
                    target = Suppose[i];
                    break;
                }
            }
            return target;
        }
        // 剔除重复的点
        protected poi_dic Remove(List<poi_dic> raw)
        {
            poi_dic result = new poi_dic();
            for (int i = 0; i < raw.Count; i++)
            {
                foreach (KeyValuePair<string, Point2D> item in raw[i])
                {
                    if (!result.ContainsKey(item.Key))
                        result.Add(item.Key, item.Value);
                }
            }
            return result;
        }
        //计算站心坐标
        protected List<poi_dic> Polar(List<Sta> AllObs)
        {
            List<poi_dic> localData = new List<poi_dic>();
            for (int i = 0; i < AllObs.Count; i++)
            {
                poi_dic fake = new poi_dic
                {
                    { AllObs[i].name, new Point2D(0, 0) }//初始化测站点坐标，以推算其他点
                };//储存单测站站心坐标
                double a0 = 0;//测站第一个照准方向的观测值
                //int i = 0;
                for (int j = 0; j < AllObs[i].data.Count; j++)
                {
                    double angle, dis;
                    var data = AllObs[i].data[j];
                    if (AngleExist(i, j) == 0) continue;
                    if (Dis(i,j)==0) continue;

                    dis = data.distance;
                    if (j == 0)
                    {
                        angle = 0;
                        a0 = DMS2RD(data.angle);
                    }
                    else
                        angle = Keep_0To2Pi(DMS2RD(data.angle) - a0);

                    Point2D p = new Point2D
                    {
                        X = dis * Math.Cos(angle),
                        Y = dis * Math.Sin(angle)
                    };//初始化照准点
                    fake.Add(data.name, p);
                }
                localData.Add(fake);
            }
            return localData;
        }
        //测角三角网近似坐标
        protected void Approx_Triangle()
        {
            foreach (KeyValuePair<string, Point2D> p in knownPoint)
            {
                if (!Approx.ContainsKey(p.Key))
                    Approx.Add(p.Key, p.Value);
            }
            int iter = 0;//循环计算的次数,有可能一次计算不完
            int sign;
            string Aname, Bname, Pname;
            while (true)
            {
                int i = 0;
                foreach (KeyValuePair<string, Dictionary<string, double>> sta in obsL)
                {
                    int j = 0, k = 0;
                    Aname = AllObs[i].name;
                    foreach (KeyValuePair<string, double> obs in sta.Value)
                    {
                        if (j == sta.Value.Count - 1) k = 0;
                        else k = j + 1;
                        Bname = AllObs[i].data[j].name;
                        Pname = AllObs[i].data[k].name;

                        if (Aim(Pname, Bname) == 0 && Aim(Bname, Pname) == 0)
                        { //ABP能否组成三角形；A照准B、P，一定连通，只需判断BP
                            j++; continue;
                        }
                        int kn = 0;
                        if (Approx.ContainsKey(Aname)) kn++;
                        if (Approx.ContainsKey(Bname)) kn++;
                        if (Approx.ContainsKey(Pname)) kn++;
                        if (kn == 2)//两个点已知，一个点未知才能计算
                        {
                            double beta = 0, alpha = 0, gama = 0;

                            alpha = Angle_ijk(Aname,Bname, Pname);
                            if (Aim(Bname, Aname) == 1 && Aim(Bname, Pname) == 1)
                                beta = Angle_ijk(Bname, Pname, Aname);
                            if (Aim(Pname, Aname) == 1 && Aim(Pname, Bname) == 1)
                                gama = Angle_ijk(Pname, Aname, Bname);

                            int an = 0;
                            if (alpha != 0) an++;
                            if (beta != 0) an++;
                            if (gama != 0) an++;
                            if (an < 2) { j++; continue; }

                            if (!Approx.ContainsKey(Pname))
                            {
                                sign = 1;//相邻照准点夹角大于180，ABP逆时针排列（一般不会）
                                if (alpha > Math.PI) { alpha = 2 * Math.PI - alpha; sign = -1; }
                                if (beta > Math.PI) { beta = 2 * Math.PI - beta; sign = -1; }
                                if (gama > Math.PI) { gama = 2 * Math.PI - gama; sign = -1; }
                                if (alpha == 0) alpha = Math.PI - beta - gama;
                                if (beta == 0) beta = Math.PI - alpha - gama;
                                if (alpha < 1e-4 || beta < 1e-4) { j++; continue; }   //避免极端情况
                                Point2D A = Approx[Aname];
                                Point2D B = Approx[Bname];
                                Point2D P = Forward_Intersection(A, B, alpha, beta, sign);
                                Approx.Add(Pname, P);
                            }
                            else if (!Approx.ContainsKey(Bname))
                            {
                                sign = 1;
                                if (alpha > Math.PI) { alpha = 2 * Math.PI - alpha; sign = -1; }
                                if (beta > Math.PI) { beta = 2 * Math.PI - beta; sign = -1; }
                                if (gama > Math.PI) { gama = 2 * Math.PI - gama; sign = -1; }
                                if (gama == 0) gama = Math.PI - alpha - beta;
                                if (alpha == 0) alpha = Math.PI - gama - beta;
                                if (alpha < 1e-4 || gama < 1e-4) { j++; continue; }
                                Point2D P = Approx[Pname];
                                Point2D A = Approx[Aname];
                                Point2D B = Forward_Intersection(P, A, gama, alpha, sign);
                                Approx.Add(Bname, B);
                            }
                            else
                            {
                                sign = 1;
                                if (alpha > Math.PI) { alpha = 2 * Math.PI - alpha; sign = -1; }
                                if (beta > Math.PI) { beta = 2 * Math.PI - beta; sign = -1; }
                                if (gama > Math.PI) { gama = 2 * Math.PI - gama; sign = -1; }
                                if (beta == 0) beta = Math.PI - alpha - gama;
                                if (gama == 0) gama = Math.PI - alpha - beta;
                                if (beta < 1e-4 || gama < 1e-4) { j++; continue; }
                                Point2D B = Approx[Bname];
                                Point2D P = Approx[Pname];
                                Point2D A = Forward_Intersection(B, P, beta, gama, sign);
                                Approx.Add(Aname, A);
                            }
                        }
                        j++;
                    }
                    i++;
                }
                iter++;
                if (Approx.Count == PointName.Count || iter == 10) //设置跳出循环的条件，同时避免死循环
                    break;
            }
        }
        //测边网近似坐标
        protected void Approx_Trilateral()
        {
            foreach (KeyValuePair<string, Point2D> p in knownPoint)
            {
                if (!Approx.ContainsKey(p.Key))
                    Approx.Add(p.Key, p.Value);
            }
            int iter = 0;
            int sign = 1;
            string Aname, Bname, Pname;
            while (true)
            {
                int i = 0;
                foreach (KeyValuePair<string, Dictionary<string, double>> sta in obsS)
                {
                    Aname = AllObs[i].name;
                    int j = 0, k = 0;
                    foreach (KeyValuePair<string, double> obs in sta.Value)
                    {
                        if (j == sta.Value.Count - 1) k = 0;
                        else k = j + 1;
                        Bname = AllObs[i].data[j].name;
                        Pname = AllObs[i].data[k].name;
#if false
                        //A未知时，BP不必连通
                        if (Aim(Pname, Bname) == 0 && Aim(Bname, Pname) == 0)
                        { //ABP能否组成三角形；A照准B、P，一定连通，只需判断BP
                            j++; continue;
                        }
#endif
                        int kn = 0;
                        if (Approx.ContainsKey(Aname)) kn++;
                        if (Approx.ContainsKey(Bname)) kn++;
                        if (Approx.ContainsKey(Pname)) kn++;

                        if (kn == 2)//两个点已知，一个点未知才能计算
                        {
                            double dis_ab = 0, dis_ap = 0, dis_bp = 0;

                            dis_ab = Dis(Aname, Bname);
                            if (dis_ab == 0 && Aim(Bname, Aname) == 1)
                                dis_ab = Dis(Bname, Aname);
                            dis_ap = Dis(Aname, Pname);
                            if (dis_ap == 0 && Aim(Pname, Aname) == 1)
                                dis_ap = Dis(Pname, Aname);
                            if (Aim(Bname, Pname) == 1)
                                dis_bp = Dis(Bname,Pname );
                            if (dis_bp == 0 && Aim(Pname, Bname) == 1)
                                dis_bp = Dis(Pname, Bname);

                            if (!Approx.ContainsKey(Pname))
                            {
                                if (dis_ap != 0 && dis_bp != 0)
                                {
                                    Point2D A = Approx[Aname];
                                    Point2D B = Approx[Bname];
                                    Point2D P = Trilateral_Intersection(A, B, dis_ap, dis_bp, sign);
                                    Approx.Add(Pname, P);
                                }
                            }
                            else if (!Approx.ContainsKey(Bname))
                            {
                                if (dis_ab != 0 && dis_bp != 0)
                                {
                                    Point2D P = Approx[Pname];
                                    Point2D A = Approx[Aname];
                                    Point2D B = Trilateral_Intersection(P, A, dis_bp, dis_ab, sign);
                                    Approx.Add(Bname, B);
                                }
                            }
                            else
                            {
                                if (dis_ab != 0 && dis_ap != 0)
                                {
                                    Point2D B = Approx[Bname];
                                    Point2D P = Approx[Pname];
                                    Point2D A = Trilateral_Intersection(B, P, dis_ab, dis_ap, sign);
                                    Approx.Add(Aname, A);
                                }
                            }
                        }
                        j++;
                    }
                    i++;
                }
                iter++;
                if (Approx.Count == PointName.Count || iter == 10) //设置跳出循环的条件，同时避免死循环
                    break;
            }
        }
        //固定测站，由已知测站推算未知测站，逐测站坐标转换
        protected void ApproxFix_Trans()
        {
            foreach (KeyValuePair<string, Point2D> p in knownPoint)
            {
                if (!Approx.ContainsKey(p.Key))
                    Approx.Add(p.Key, p.Value);
            }
            var localData = Polar(AllObs);      //站心坐标
            int num = 0;//迭代计算的次数
            while (true)
            {
                num++;
                for (int i = 0; i < localData.Count; i++)
                {
                    int k = 0;
                    var local = localData[i];
                    var knApprox = new poi_dic();
                    //在当前测站找到所有计算出近似坐标的点
                    foreach (KeyValuePair<string, Point2D> p in local)
                    {
                        if (Approx.ContainsKey(p.Key))
                        {
                            knApprox.Add(p.Key, Approx[p.Key]);
                            k++;
                        }
                    }
                    if (k == local.Count || k < 2)
                        continue;//计算完了 or 没有足够的已知点
                    if (k >= 2)
                    {
                        var result = Trans(knApprox, local);
                        foreach (KeyValuePair<string, Point2D> p in result)
                        {
                            if (!Approx.ContainsKey(p.Key))
                                Approx.Add(p.Key, p.Value);
                        }
                    }
                }
                if (Approx.Count == PointName.Count)
                    break;
                if (num > 10)  //防止死循环
                    break;
            }
        }
        //不分区无定向自由测站边角交会网
        //ref [2]
        protected void ApproxFree()
        {
            //由站心坐标转换得到的假设坐标
            var Suppose_raw = new List<poi_dic>();
            //剔除重复点的假设坐标
            var suppose = new poi_dic();

            var localData = Polar(AllObs); //站心坐标
            //第一测站的数据
            poi_dic result = localData[0];
            Suppose_raw.Add(result);
            int num = 0;
            while (true)
            {
                num++;
                for (int i = 1; i < localData.Count; i++)
                {
                    poi_dic next = localData[i];
                    int exist_ukp = 0;
                    foreach (KeyValuePair<string, Point2D> p in next)
                    {
                        if (!Approx.ContainsKey(p.Key))
                        {
                            exist_ukp = 1;//当前测站存在未知点
                            break;
                        }
                    }
                    if (exist_ukp != 1)
                        continue;
                    var target = Connect(next, Suppose_raw);//在suppose中找next关联的测站
                    if (target.Count >= 2) //找到了
                    {
                        result = Trans(target, next);//next转换后的坐标
                        Suppose_raw.Add(result);
                    }
                }
                suppose = Remove(Suppose_raw);//剔除重复的点位,顺便转换格式
                if (suppose.Count == PointName.Count)
                    break;
                if (num > 50)
                    break;
            }
            Approx = Trans(knownPoint, suppose);
            //Approx = Remove(Approx_raw);
        }
        //分区无定向导线
        protected void ApproxFree_FQ()
        {
            //未剔除重复的概略坐标点
            List<poi_dic> Approx_raw = new List<poi_dic>();
            //由站心坐标转换得到的假设坐标
            List<poi_dic> Suppose_raw = new List<poi_dic>();

            var localData = Polar(AllObs);                 //站心坐标
            poi_dic result = localData[0];            //第一测站的数据
            Suppose_raw.Add(result);
            int num = 0;
            while (true)
            {
                num++;
                for (int i = 1; i < localData.Count; i++)
                {
                    poi_dic next = localData[i];
                    int exist_ukp = 0;
                    foreach (KeyValuePair<string, Point2D> p in next)
                    {
                        if (!Approx.ContainsKey(p.Key))
                        {
                            exist_ukp = -1;//当前测站存在未知点
                            break;
                        }
                    }
                    if (exist_ukp != -1)
                        continue;
                    var target = Connect(next, Suppose_raw);//在suppose中找next关联的测站
                    if (target.Count >= 2) //找到了
                    {
                        result = Trans(target, next);//next转换后的坐标
                        Suppose_raw.Add(result);
                        var suppose = Remove(Suppose_raw);//剔除重复的点位,顺便转换格式
                        int count = 0;//记录测站组包含已知点的个数
                        foreach (KeyValuePair<string, Point2D> pair in suppose)
                        {
                            if (knownPoint.ContainsKey(pair.Key))
                                count++;
                        }
                        if (count < 2) //同样，与已知点做坐标转换，同样需要至少两个相同点
                            continue;
                        else
                        {
                            var poi = Trans(knownPoint, suppose);
                            Approx_raw.Add(poi);
                            Suppose_raw.Clear();//清空一个区段的测站数据，继续下一次转换
                            if (i < localData.Count - 1)
                            {
                                result = localData[i + 1];//从下一个测站重新开始坐标转换
                                Suppose_raw.Add(result);
                                i++;
                            }
                        }
                    }
                }
                Approx = Remove(Approx_raw);
                if (Approx.Count == PointName.Count)
                    break;
                if (num > 20)
                    break;
            }
        }
        //固定测站，已知点推算未知点，单边推算
        // ref [1]
        protected void ApproxFix(Proc_t popt,ref File_t fopt, List<Sta> AllObs, int MaxCount)
        {
            List<Triangle_t> S3 = new List<Triangle_t>();
            if (File.Exists(fopt.approx_3s))
                ReadApprox_3S(fopt.approx_3s, S3);
            if (popt.AdjOpt == ADJOPT.Constriant)
            {
                foreach (KeyValuePair<string, Point2D> p in knownPoint)
                {
                    if (!Approx.ContainsKey(p.Key))
                        Approx.Add(p.Key, p.Value);
                }
            }
            int count = 0;
            
            while (true)
            {
                for (int i = 0; i < AllObs.Count; i++)
                {
                    string staName = AllObs[i].name;
                    int ks = 0;
                    List<int> AimIndex = new List<int>(); //存储已经计算出的照准点编号
                    if (Approx.ContainsKey(staName))
                        ks++;
                    for (int j = 0; j < AllObs[i].data.Count; j++)
                    {
                        if (Approx.ContainsKey(AllObs[i].data[j].name))
                        {
                            ks++;
                            AimIndex.Add(j);
                        }
                    }
                    //如果测站内全部已知
                    if (ks == AllObs[i].data.Count + 1)
                        continue;

                    CalAzi(staName, AimIndex, Azis);
                    var data = AllObs[i].data;
                    if (Approx.ContainsKey(staName))
                    {
                        string Aname = staName;
                        // ①遍历每个方向
                        for(int j = 0; j < data.Count; j++)
                        {
                            string Pname = data[j].name;
                            if (Approx.ContainsKey(Pname)) continue;
                            double dis_ap = Dis(Aname, Pname);
                            if (dis_ap == 0 && Aim(Pname, Aname) == 1)
                                dis_ap = Dis(Pname, Aname);
                            int idx = FindAzi(Aname, Pname, Azis);
                            if(dis_ap!=0 && idx != -1)
                            {
                                double azi_ap = Azis[idx].azi;
                                double x = Approx[Aname].X + dis_ap * Math.Cos(azi_ap);
                                double y = Approx[Aname].Y + dis_ap * Math.Sin(azi_ap);
                                Approx.Add(Pname, new Point2D(x, y));
                            }
                        }
                        // ②构造三角形ABP，以A为测站，AB->AP是顺时针相邻的照准方向
                        for (int j = 0; j < data.Count; j++)
                        {
                            double dis_ap = 0, dis_bp = 0;
                            double alpha = 0, beta = 0, gamma = 0;

                            int k = 0;
                            if (j == data.Count - 1) k = 0;
                            else k = j + 1;
                            string Bname = data[j].name;
                            string Pname = data[k].name;

                            int again = 0;
                            double dx = 0, dy = 0, azi_ab = 0;
                            if (Approx.ContainsKey(Bname) && (!Approx.ContainsKey(Pname)))
                            {
                                // 已知AB，求P
                                again = 1;
                                dx = Approx[Bname].X - Approx[Aname].X;
                                dy = Approx[Bname].Y - Approx[Aname].Y;
                                azi_ab = GetAzi(dx, dy);
                            }

                            dis_ap = Dis(Aname, Pname);
                            if (dis_ap == 0 && Aim(Pname, Aname) == 1)
                                dis_ap = Dis(Pname, Aname);
                            if (Aim(Bname, Pname) == 1)
                                dis_bp = Dis(Bname, Pname);
                            if (dis_bp == 0 && Aim(Pname, Bname) == 1)
                                dis_bp = Dis(Pname, Bname);

                            alpha = Angle_ijk(Aname, Bname, Pname);
                            if (Aim(Bname, Aname) == 1 && Aim(Bname, Pname) == 1)
                                beta = Angle_ijk(Bname, Pname, Aname);
                            if (Aim(Pname, Aname) == 1 && Aim(Pname, Bname) == 1)
                                gamma = Angle_ijk(Pname, Aname, Bname);

                            int an = 0;
                            if (alpha != 0) an++;
                            if (beta != 0) an++;
                            if (gamma != 0) an++;

                            if (dis_ap != 0 && alpha != 0 && again==1)
                            {
                                double azi_ap = Keep_0To2Pi(azi_ab + alpha); 
                                double x = Approx[Aname].X + dis_ap * Math.Cos(azi_ap);
                                double y = Approx[Aname].Y + dis_ap * Math.Sin(azi_ap);
                                Approx.Add(Pname, new Point2D(x, y));
                                continue;
                            }
                            if (dis_bp != 0 && beta != 0 && again==1)
                            {
                                double azi_bp = Keep_0To2Pi(azi_ab + Math.PI - beta); 
                                double x = Approx[Bname].X + dis_bp * Math.Cos(azi_bp);
                                double y = Approx[Bname].Y + dis_bp * Math.Sin(azi_bp);
                                Approx.Add(Pname, new Point2D(x, y));
                                continue;
                            }
                            if (an >= 2 && again==1)
                            {
                                int sign = 1; //sign =-1 : 相邻照准点夹角大于180，ABP逆时针排列（一般不会）
                                if (alpha > Math.PI || beta > Math.PI || gamma > Math.PI)
                                    sign = -1;
                                alpha = Keep_0ToPi(alpha);
                                beta = Keep_0ToPi(beta);
                                gamma = Keep_0ToPi(gamma);

                                //A,B对P前方交会时，gamma不能太小，即▲ABP不能太陡，
                                //不然alpha,beta产生的坐标偏差很大，测距越短，偏差越小
                                //前方交会的gamma的最小阈值设为10°时,1KM的测距产生的偏差大约3cm
                                if (gamma == 0) gamma = Math.PI - alpha - beta;
                                if (gamma < D2RD(10))
                                    continue;
                                if (alpha == 0) alpha = Math.PI - beta - gamma;
                                if (beta == 0) beta = Math.PI - alpha - gamma;
                                //避免极端情况，alpha,beta在0附近时，ABP的顺逆时针容易颠倒
                                if (alpha < 1e-4 || beta < 1e-4) { continue; }

                                Point2D A = Approx[Aname];
                                Point2D B = Approx[Bname];
                                Point2D P = Forward_Intersection(A, B, alpha, beta, sign);
                                Approx.Add(Pname, P);
                                continue;
                            }
                            if (dis_ap != 0 && dis_bp != 0 && popt.Approx_Trilateral != 0)
                            {
                                int sign = FindApprox_3S(Aname,Bname,Pname,S3);
                                if (sign != 0 && again==1)
                                {
                                    Point2D A = Approx[Aname];
                                    Point2D B = Approx[Bname];
                                    Point2D P = Trilateral_Intersection(A, B, dis_ap, dis_bp, sign);
                                    if (P.valid == 1)
                                    {
                                        Approx.Add(Pname, P);
                                        continue;
                                    }
                                }
                                if (an != 0 && again==1)   
                                {
                                    sign = 1;
                                    if (alpha > Math.PI || beta > Math.PI || gamma > Math.PI)
                                        sign = -1;
                                    Point2D A = Approx[Aname];
                                    Point2D B = Approx[Bname];
                                    Point2D P = Trilateral_Intersection(A, B, dis_ap, dis_bp, sign);
                                    if (P.valid == 1)
                                    {
                                        Approx.Add(Pname, P);
                                        continue;
                                    }
                                }
                                else if(an==0 && again==1)
                                {
                                    // 可以先假设顺时针，再利用已知信息判断
                                    Point2D A = Approx[Aname];
                                    Point2D B = Approx[Bname];
                                    Point2D P = Trilateral_Intersection(A, B, dis_ap, dis_bp, 1);
                                    double bias = 0;
                                    sign = 1;
                                    int pt = PointTrue(Pname, P, ref bias);
                                    if (pt != -1)
                                    {
                                        if (pt == 0)
                                        {
                                            bias = 0;
                                            P = Trilateral_Intersection(A, B, dis_ap, dis_bp, -1);
                                            pt = PointTrue(Pname, P, ref bias);
                                            sign = -1;
                                        }
                                        if (pt == 1)
                                        {
                                            if (P.valid == 1)
                                            {
                                                Approx.Add(Pname, P);
                                                continue;
                                            }
                                        }
                                    }
                                    else
                                    {
                                        Approx_3S(Aname, Bname, Pname,0, S3);
                                    }
                                }
                                else
                                {
                                    Approx_3S(Aname, Bname, Pname,0, S3);
                                }
                            }
                        }
                    }
                    if (!Approx.ContainsKey(staName))
                    {
                        string Pname = staName;
                        // ① 遍历所有方向，以P（i）为测站，A（j）为照准点
                        for(int j = 0; j < data.Count; j++)
                        {
                            string Aname = data[j].name;
                            if (!Approx.ContainsKey(Aname)) continue;
                            double dis_ap = Dis(Aname, Pname);
                            if (dis_ap == 0 && Aim(Pname, Aname) == 1)
                                dis_ap = Dis(Pname, Aname);
                            int idx = FindAzi(Aname, Pname, Azis);
                            if(dis_ap!=0 && idx != -1)
                            {
                                double azi_ap = Azis[idx].azi;
                                double x = Approx[Aname].X + dis_ap * Math.Cos(azi_ap);
                                double y = Approx[Aname].Y + dis_ap * Math.Sin(azi_ap);
                                Approx.Add(Pname, new Point2D(x, y));
                                break;
                            }
                        }
                        // ② 构造三角形ABP，以P（i）为测站，依次照准A,B (j,k)
                        for (int j = 0; j < data.Count; j++)
                        {
                            double dis_pb = 0, dis_pa = 0;
                            double alpha = 0, beta = 0, gamma = 0;

                            int k = 0;
                            if (j == data.Count - 1) k = 0;
                            else k = j + 1;

                            string Aname = data[j].name;
                            string Bname = data[k].name;

                            int again = 0;
                            double dx = 0, dy = 0, azi_ab = 0;
                            if (!Approx.ContainsKey(Pname) && Approx.ContainsKey(Aname) && 
                                Approx.ContainsKey(Bname))
                            {
                                again = 1;
                                dx = Approx[Bname].X - Approx[Aname].X;
                                dy = Approx[Bname].Y - Approx[Aname].Y;
                                azi_ab = GetAzi(dx, dy);
                            }

                            if (Aim(Aname, Bname) == 0 && Aim(Bname, Aname) == 0)
                                continue;

                            dis_pa = Dis(Pname, Aname);
                            if (dis_pa == 0 && Aim(Aname, Pname) == 1)
                                dis_pa = Dis(Aname, Pname);
                            dis_pb = Dis(Pname, Bname);
                            if (dis_pb == 0 && Aim(Bname, Pname) == 1)
                                dis_pb = Dis(Bname, Pname);

                            gamma = Angle_ijk(Pname, Aname, Bname);
                            if (Aim(Aname, Pname) == 1 && Aim(Aname, Bname) == 1)
                                alpha = Angle_ijk(Aname, Bname, Pname);
                            if (Aim(Bname, Pname) == 1 && Aim(Bname, Aname) == 1)
                                beta = Angle_ijk(Bname, Pname, Aname);

                            int an = 0;  //三角形内角个数
                            if (gamma != 0) an++;
                            if (alpha != 0) an++;
                            if (beta != 0) an++;

                            if (dis_pa != 0 && alpha != 0 && again==1)  //LS推算
                            {
                                double azi_ap = Keep_0To2Pi(azi_ab + alpha); //azi_ap = azi_ab + ∠PAB
                                double x = Approx[Aname].X + dis_pa * Math.Cos(azi_ap);
                                double y = Approx[Aname].Y + dis_pa * Math.Sin(azi_ap);
                                Approx.Add(Pname, new Point2D(x, y));
                                break;
                            }
                            if (dis_pb != 0 && beta != 0 && again==1) //LS推算
                            {
                                double azi_bp = Keep_0To2Pi(azi_ab + Math.PI - beta);
                                double x = Approx[Bname].X + dis_pb * Math.Cos(azi_bp);
                                double y = Approx[Bname].Y + dis_pb * Math.Sin(azi_bp);
                                Approx.Add(Pname, new Point2D(x, y));
                                break;
                            }
                            if (an >= 2 && again==1)  //测角前方交会
                            {
                                int sign = 1;//相邻照准点夹角大于180，ABP逆时针排列
                                if (alpha > Math.PI || beta > Math.PI || gamma > Math.PI)
                                    sign = -1;
                                alpha = Keep_0ToPi(alpha); //三角形内角小于180°
                                beta = Keep_0ToPi(beta);
                                gamma = Keep_0ToPi(gamma);

                                if (gamma == 0) gamma = Math.PI - alpha - beta;
                                if (gamma < D2RD(10))
                                    continue;
                                if (alpha == 0) alpha = Math.PI - gamma - beta;
                                if (beta == 0) beta = Math.PI - gamma - alpha;
                                if (alpha < 1e-4 || beta < 1e-4) { continue; } //避免极端情况

                                Point2D A = Approx[Aname];
                                Point2D B = Approx[Bname];
                                Point2D P = Forward_Intersection(A, B, alpha, beta, sign);
                                Approx.Add(Pname, P);
                                break;
                            }
                            //测距交会
                            if (dis_pa != 0 && dis_pb != 0 && popt.Approx_Trilateral != 0) 
                            {
                                int sign = FindApprox_3S(Aname, Bname, Pname, S3);
                                if (sign != 0 && again==1)
                                {
                                    Point2D A = Approx[Aname];
                                    Point2D B = Approx[Bname];
                                    Point2D P = Trilateral_Intersection(A, B, dis_pa, dis_pb, sign);
                                    if (P.valid == 1)
                                    {
                                        Approx.Add(Pname, P);
                                        break;
                                    }
                                }
                                if (an != 0 && again==1)
                                {
                                    sign = 1;
                                    if (alpha > Math.PI || beta > Math.PI || gamma > Math.PI)
                                        sign = -1;  //（ABP逆时针）只要知道一个角度，就能判断顺逆
                                    Point2D A = Approx[Aname];
                                    Point2D B = Approx[Bname];
                                    Point2D P = Trilateral_Intersection(A, B, dis_pa, dis_pb, sign);
                                    if (P.valid == 1)
                                    {
                                        Approx.Add(Pname, P);
                                        break;
                                    }
                                }
                                else
                                {
                                    Approx_3S(Aname, Bname, Pname,0, S3);
                                }

                            }
                        }
                        int ok = 0;
                        if (Approx.ContainsKey(Pname)) { ok = 1; }
                        // ③以上Aname和Bname是相邻的照准点，以下可能不相邻；
                        for (int j = 0; j < data.Count; j++)  //后方交会，方位角交会
                        {
                            for (int k = j + 1; k < data.Count; k++)  //后方交会和方位角交会
                            {
                                //以P为测站，依次照准A,B,C (j,k,m)
                                //double alpha = 0, beta = 0, gama = 0;
                                string Aname = data[j].name;
                                string Bname = data[k].name;

                                if (Approx.ContainsKey(Pname))
                                    continue;
                                if (!Approx.ContainsKey(Aname) || !Approx.ContainsKey(Bname))
                                    continue;

                                double dis_pa, dis_pb;
                                dis_pa = Dis(Pname, Aname);
                                if (dis_pa == 0 && Aim(Aname, Pname) == 1)
                                    dis_pa = Dis(Aname, Pname);
                                dis_pb = Dis(Pname, Bname);
                                if (dis_pb == 0 && Aim(Bname, Pname) == 1)
                                    dis_pb = Dis(Bname, Pname);

                                if (dis_pa != 0 && dis_pb != 0 && popt.Approx_Trilateral != 0)  //测距交会
                                {
                                    double gamma = Angle_ijk(Pname, Aname, Bname);
                                    Point2D A = Approx[Aname];
                                    Point2D B = Approx[Bname];
                                    Point2D P = new Point2D();
                                    if (gamma != 0)
                                    {
                                        int sign = 1;
                                        if (gamma > Math.PI) 
                                            sign = -1;      //ABP逆时针，如果gamma不知道，无法判断顺逆
                                        P = Trilateral_Intersection(A, B, dis_pa, dis_pb, sign);
                                        if (P.valid == 1)
                                        {
                                            Approx.Add(Pname, P);
                                            ok = 1; break;
                                        }
                                    }
                                    else
                                    {
                                        double bias = 0;
                                        P = Trilateral_Intersection(A, B, dis_pa, dis_pb, 1);
                                        int pt = PointTrue(Pname, P, ref bias);
                                        if (pt != -1)
                                        {
                                            if (pt == 0)
                                            {
                                                bias = 0;
                                                P = Trilateral_Intersection(A, B, dis_pa, dis_pb, -1);
                                                pt = PointTrue(Pname, P, ref bias);
                                            }
                                            if (pt == 1)
                                            {
                                                if (P.valid == 1)
                                                {
                                                    Approx.Add(Pname, P);
                                                    ok = 1; break;
                                                }
                                            }
                                        }
                                    }
                                }
                                for (int m = k + 1; m < data.Count; m++)  //后方交会
                                {
                                    if (ok == 1) break;
                                    double alpha = 0, beta = 0, gamma = 0;
                                    string Cname = data[m].name;
                                    if (!Approx.ContainsKey(Cname)) continue;

                                    alpha = Angle_ijk(i, k, m);
                                    beta = Angle_ijk(i, m, j);
                                    gamma = Angle_ijk(i, j, k);
                                    if (alpha == 0 || beta == 0 || gamma == 0) continue;

                                    double azi_ab = 0, azi_ba = 0, azi_bc = 0, azi_cb = 0, azi_ac = 0, azi_ca = 0;
                                    Point2D A = Approx[Aname];
                                    Point2D B = Approx[Bname];
                                    Point2D C = Approx[Cname];
                                    azi_ab = GetAzi(A, B);
                                    azi_ba = GetAzi(B, A);
                                    azi_bc = GetAzi(B, C);
                                    azi_cb = GetAzi(C, B);
                                    azi_ac = GetAzi(A, C);
                                    azi_ca = GetAzi(C, A);

                                    double aga = 0, agb = 0, agc = 0;
                                    aga = Keep_0To2Pi(azi_ac - azi_ab);
                                    agb = Keep_0To2Pi(azi_ba - azi_bc);
                                    agc = Keep_0To2Pi(azi_cb - azi_ca);
                                    if (aga == 0 || agb == 0 || agc == 0) continue;//避免值不存在


                                    double Pa = 0, Pb = 0, Pc = 0;
                                    if (Math.Abs(aga - alpha) > 1e-10) { alpha += 1e-6; } //如果aga=alpha,避免Pa算不出
                                    Pa = 1 / (Cot(aga) - Cot(alpha));  //即使测距为1KM，1e-6产生的点位偏差也才1mm
                                    if (Math.Abs(agb - beta) > 1e-10) { beta += 1e-6; }
                                    Pb = 1 / (Cot(agb) - Cot(beta));
                                    if (Math.Abs(agc - gamma) > 1e-10) { gamma += 1e-6; }
                                    { Pc = 1 / (Cot(agc) - Cot(gamma)); }
                                    Point2D P = Backward_Intersection(A, B, C, Pa, Pb, Pc);
                                    Approx.Add(Pname, P);
                                    ok = 1;
                                    break;
                                }

                                if (ok == 0)  //方位角交会(不管AB是否连通，都可以计算）
                                {
                                    double alpha = 0, beta = 0, gamma = 0;
                                    double dx = Approx[Bname].X - Approx[Aname].X;
                                    double dy = Approx[Bname].Y - Approx[Aname].Y;
                                    double azi_ab = GetAzi(dx, dy);

                                    int idz_pa = FindAzi(Pname, Aname, Azis);
                                    int idz_ap = FindAzi(Aname, Pname, Azis);
                                    int idz_pb = FindAzi(Pname, Bname, Azis);
                                    int idz_bp = FindAzi(Bname, Pname, Azis);
                                    gamma = Angle_ijk(i, j, k);
                                    int sign = 1;
                                    if (gamma > Math.PI) { sign = -1; }
                                    if (idz_pa != -1 || idz_ap != -1)
                                    {
                                        if (idz_ap != -1) alpha = Azis[idz_ap].azi - azi_ab;
                                        else alpha = Azis[idz_pa].azi + Math.PI - azi_ab;
                                        alpha = Keep_0To2Pi(alpha * sign);
                                    }
                                    if (idz_pb != -1 || idz_bp != -1)
                                    {
                                        if (idz_bp != -1) beta = (azi_ab + Math.PI - Azis[idz_bp].azi);
                                        else beta = (azi_ab - Azis[idz_pb].azi);
                                        beta = Keep_0To2Pi(beta * sign);
                                    }

                                    int an0 = 0;  //三角形内角个数
                                    if (alpha != 0) an0++;
                                    if (beta != 0) an0++;
                                    if (gamma != 0) an0++;

                                    if (an0 >= 2)
                                    {
                                        if (alpha > Math.PI) { alpha = 2 * Math.PI - alpha; }
                                        if (beta > Math.PI) { beta = 2 * Math.PI - beta; }
                                        if (gamma > Math.PI) { gamma = 2 * Math.PI - gamma; }

                                        if (gamma == 0) gamma = Math.PI - alpha - beta;
                                        if (gamma < D2RD(10))
                                            continue;
                                        if (alpha == 0) alpha = Math.PI - beta - gamma;
                                        if (beta == 0) beta = Math.PI - alpha - gamma;
                                        if (alpha < 1e-4 || beta < 1e-4) { continue; }

                                        Point2D A = Approx[Aname];
                                        Point2D B = Approx[Bname];
                                        Point2D P = Forward_Intersection(A, B, alpha, beta, sign);
                                        Approx.Add(Pname, P);
                                        ok = 1;
                                        break;
                                    }
                                }
                                if (ok == 1) break;
                            }
                            if (ok == 1) break;
                        }

                        if (ok == 0)  //一边一角交会，穷举法
                        {
                            Point2D P = new Point2D();
                            if (LoopCircle(Pname, ref P) == 1)
                            {
                                Approx.Add(Pname, P);
                                ok = 1;
                            }
                        }
                    }
                    if (Approx.Count >= MaxCount)
                        break;
                    CalAzi(staName, AimIndex, Azis);
                }
                if (Approx.Count >= MaxCount)
                    break;
                count++;
                if (count == MaxCount)
                    break;
            }
            for(int i = 0; i < PointName.Count; i++)
            {
                if (!Approx.ContainsKey(PointName[i]))
                    Print(PointName[i]);
            }
            List<string> trilateral = new List<string>();
            trilateral.Add("# 测边交会二义性文件");
            trilateral.Add("# A,B,P,sign (0=未知,1=顺时针,-1=逆时针)");
            for(int i = 0; i < S3.Count; i++)
            {
                trilateral.Add(String.Format("{0},{1},{2},{3}", 
                    S3[i].Aname, S3[i].Bname, S3[i].Pname,S3[i].sign));
            }
            if (S3.Count != 0 && Approx.Count < MaxCount)
                File.WriteAllLines(fopt.approx_3s, trilateral);
            else
                fopt.approx_3s = "";
        }

        //计算近似坐标
        protected int Approximate(Proc_t popt, ref File_t fopt)
        {

            AdjustObs(AllObs);
            ApproxFree();
            ApproxFix(popt,ref fopt, AllObs, PointName.Count);

            if (Approx.Count < PointName.Count)
            {
                Print("近似坐标计算失败！");
                return 0;
            }

            foreach (KeyValuePair<string, Point2D> p in knownPoint)
            {
                if (Approx.ContainsKey(p.Key))
                    Approx[p.Key] = p.Value;
            }

            CopyApprox = CopyDic(Approx);
            return 1;
        }
    }
}
